/* 
 * File:   MathUtil.cpp
 * Author: Pie
 * 
 * Created on April 22, 2011, 7:28 PM
 */


#include "MathUtil.h"
#include <math.h>
#ifdef DEBUG_NEW
#include "debug/debug_new.h"
#endif


//
//float clampf(float c, float min, float max) {
//    return c < min ? min : (c > max ? max : c);
//}

void setIdentity(aiMatrix4x4* mat) {
    mat->a1 = 1.0f;
    mat->a2 = 0.0f;
    mat->a3 = 0.0f;
    mat->a4 = 0.0f;

    mat->b1 = 0.0f;
    mat->b2 = 1.0f;
    mat->b3 = 0.0f;
    mat->b4 = 0.0f;

    mat->c1 = 0.0f;
    mat->c2 = 0.0f;
    mat->c3 = 1.0f;
    mat->c4 = 0.0f;

    mat->d1 = 0.0f;
    mat->d2 = 0.0f;
    mat->d3 = 0.0f;
    mat->d4 = 1.0f;
}

void getInverseV(aiMatrix4x4* mat, double* v) {
    float det = mat->Determinant();
    if (det == 0.0f) {
        return;
    }

    float a1 = mat->a1;
    float a2 = mat->a2;
    float a3 = mat->a3;
    float a4 = mat->a4;
    float b1 = mat->b1;
    float b2 = mat->b2;
    float b3 = mat->b3;
    float b4 = mat->b4;
    float c1 = mat->c1;
    float c2 = mat->c2;
    float c3 = mat->c3;
    float c4 = mat->c4;
    float d1 = mat->d1;
    float d2 = mat->d2;
    float d3 = mat->d3;
    float d4 = mat->d4;
    float invdet = 1.0f / det;

    //    v[0] = invdet * (b2 * (c3 * d4 - c4 * d3) + b3 * (c4 * d2 - c2 * d4) + b4 * (c2 * d3 - c3 * d2));
    //    v[1] = -invdet * (a2 * (c3 * d4 - c4 * d3) + a3 * (c4 * d2 - c2 * d4) + a4 * (c2 * d3 - c3 * d2));
    //    v[2] = invdet * (a2 * (b3 * d4 - b4 * d3) + a3 * (b4 * d2 - b2 * d4) + a4 * (b2 * d3 - b3 * d2));
    //    v[3] = -invdet * (a2 * (b3 * c4 - b4 * c3) + a3 * (b4 * c2 - b2 * c4) + a4 * (b2 * c3 - b3 * c2));
    //    v[4] = -invdet * (b1 * (c3 * d4 - c4 * d3) + b3 * (c4 * d1 - c1 * d4) + b4 * (c1 * d3 - c3 * d1));
    //    v[5] = invdet * (a1 * (c3 * d4 - c4 * d3) + a3 * (c4 * d1 - c1 * d4) + a4 * (c1 * d3 - c3 * d1));
    //    v[6] = -invdet * (a1 * (b3 * d4 - b4 * d3) + a3 * (b4 * d1 - b1 * d4) + a4 * (b1 * d3 - b3 * d1));
    //    v[7] = invdet * (a1 * (b3 * c4 - b4 * c3) + a3 * (b4 * c1 - b1 * c4) + a4 * (b1 * c3 - b3 * c1));
    //    v[8] = invdet * (b1 * (c2 * d4 - c4 * d2) + b2 * (c4 * d1 - c1 * d4) + b4 * (c1 * d2 - c2 * d1));
    //    v[9] = -invdet * (a1 * (c2 * d4 - c4 * d2) + a2 * (c4 * d1 - c1 * d4) + a4 * (c1 * d2 - c2 * d1));
    //    v[10] = invdet * (a1 * (b2 * d4 - b4 * d2) + a2 * (b4 * d1 - b1 * d4) + a4 * (b1 * d2 - b2 * d1));
    //    v[11] = -invdet * (a1 * (b2 * c4 - b4 * c2) + a2 * (b4 * c1 - b1 * c4) + a4 * (b1 * c2 - b2 * c1));
    //    v[12] = -invdet * (b1 * (c2 * d3 - c3 * d2) + b2 * (c3 * d1 - c1 * d3) + b3 * (c1 * d2 - c2 * d1));
    //    v[13] = invdet * (a1 * (c2 * d3 - c3 * d2) + a2 * (c3 * d1 - c1 * d3) + a3 * (c1 * d2 - c2 * d1));
    //    v[14] = -invdet * (a1 * (b2 * d3 - b3 * d2) + a2 * (b3 * d1 - b1 * d3) + a3 * (b1 * d2 - b2 * d1));
    //    v[15] = invdet * (a1 * (b2 * c3 - b3 * c2) + a2 * (b3 * c1 - b1 * c3) + a3 * (b1 * c2 - b2 * c1));

    v[0] = invdet * (b2 * (c3 * d4 - c4 * d3) + b3 * (c4 * d2 - c2 * d4) + b4 * (c2 * d3 - c3 * d2));
    v[4] = -invdet * (a2 * (c3 * d4 - c4 * d3) + a3 * (c4 * d2 - c2 * d4) + a4 * (c2 * d3 - c3 * d2));
    v[8] = invdet * (a2 * (b3 * d4 - b4 * d3) + a3 * (b4 * d2 - b2 * d4) + a4 * (b2 * d3 - b3 * d2));
    v[12] = -invdet * (a2 * (b3 * c4 - b4 * c3) + a3 * (b4 * c2 - b2 * c4) + a4 * (b2 * c3 - b3 * c2));
    v[1] = -invdet * (b1 * (c3 * d4 - c4 * d3) + b3 * (c4 * d1 - c1 * d4) + b4 * (c1 * d3 - c3 * d1));
    v[5] = invdet * (a1 * (c3 * d4 - c4 * d3) + a3 * (c4 * d1 - c1 * d4) + a4 * (c1 * d3 - c3 * d1));
    v[9] = -invdet * (a1 * (b3 * d4 - b4 * d3) + a3 * (b4 * d1 - b1 * d4) + a4 * (b1 * d3 - b3 * d1));
    v[13] = invdet * (a1 * (b3 * c4 - b4 * c3) + a3 * (b4 * c1 - b1 * c4) + a4 * (b1 * c3 - b3 * c1));
    v[2] = invdet * (b1 * (c2 * d4 - c4 * d2) + b2 * (c4 * d1 - c1 * d4) + b4 * (c1 * d2 - c2 * d1));
    v[6] = -invdet * (a1 * (c2 * d4 - c4 * d2) + a2 * (c4 * d1 - c1 * d4) + a4 * (c1 * d2 - c2 * d1));
    v[10] = invdet * (a1 * (b2 * d4 - b4 * d2) + a2 * (b4 * d1 - b1 * d4) + a4 * (b1 * d2 - b2 * d1));
    v[14] = -invdet * (a1 * (b2 * c4 - b4 * c2) + a2 * (b4 * c1 - b1 * c4) + a4 * (b1 * c2 - b2 * c1));
    v[3] = -invdet * (b1 * (c2 * d3 - c3 * d2) + b2 * (c3 * d1 - c1 * d3) + b3 * (c1 * d2 - c2 * d1));
    v[7] = invdet * (a1 * (c2 * d3 - c3 * d2) + a2 * (c3 * d1 - c1 * d3) + a3 * (c1 * d2 - c2 * d1));
    v[11] = -invdet * (a1 * (b2 * d3 - b3 * d2) + a2 * (b3 * d1 - b1 * d3) + a3 * (b1 * d2 - b2 * d1));
    v[15] = invdet * (a1 * (b2 * c3 - b3 * c2) + a2 * (b3 * c1 - b1 * c3) + a3 * (b1 * c2 - b2 * c1));

    //return v;
}

void getAxes(aiMatrix4x4* mat, aiVector3D* x, aiVector3D* y, aiVector3D* z) {
    x->Set(mat->a1, mat->b1, mat->c1);
    y->Set(mat->a2, mat->b2, mat->c2);
    z->Set(mat->a3, mat->b3, mat->c3);
}
void applyTranslation(aiMatrix4x4* mat, aiVector3D* vec) {
    mat->a4 = vec->x;
    mat->b4 = vec->y;
    mat->c4 = vec->z;
}

void applyRotationXYZ(aiMatrix4x4* mat, aiVector3D* vec) {

    float cx = cos(vec->x);
    float sx = sin(vec->x);
    float cy = cos(vec->y);
    float sy = sin(vec->y);
    float cz = cos(vec->z);
    float sz = sin(vec->z);

    float sxsy = sx*sy;
    float cxsy = cx*sy;

    aiMatrix4x4 m(cy*cz, -cy*sz, sy, 0.0f,
            sxsy*cz+cx*sz, -sxsy*sz+cx*cz, -sx*cy, 0.0f,
            -cxsy*cz+sx*sz, cxsy*sz+sx*cz, cx*cy, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f);
    *mat *= m;
}

void applyRotationX(aiMatrix4x4* mat, float a) {
    float ca = cos(a);
    float sa = sin(a);

    aiMatrix4x4 m(1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, ca, -sa, 0.0f,
            0.0f, sa, ca, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f);
    *mat *= m;
}

void applyRotationY(aiMatrix4x4* mat, float a) {
    float ca = cos(a);
    float sa = sin(a);

    aiMatrix4x4 m(ca, 0.0f, sa, 0.0f,
            0.0f, 1.0f, 0.0f, 0.0f,
            -sa, 0.0f, ca, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f);
    *mat *= m;
}
void applyRotationZ(aiMatrix4x4* mat, float a) {
    float ca = cos(a);
    float sa = sin(a);

    aiMatrix4x4 m(ca, -sa, 0.0f, 0.0f,
            sa, ca, 0.0f, 0.0f,
            0.0f, 0.0f, 1.0f, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f);
    *mat *= m;
}

void copyMatrix(aiMatrix4x4* mat, aiMatrix4x4* out) {

    out->a1 = mat->a1;
    out->a2 = mat->a2;
    out->a3 = mat->a3;
    out->a4 = mat->a4;

    out->b1 = mat->b1;
    out->b2 = mat->b2;
    out->b3 = mat->b3;
    out->b4 = mat->b4;

    out->c1 = mat->c1;
    out->c2 = mat->c2;
    out->c3 = mat->c3;
    out->c4 = mat->c4;

    out->d1 = mat->d1;
    out->d2 = mat->d2;
    out->d3 = mat->d3;
    out->d4 = mat->d4;
}

//void printMatrix(aiMatrix4x4* mat) {
//    std::cout << mat->a1 << ", " << mat->a2 << ", " << mat->a3 << ", " << mat->a4 << "\n";
//    std::cout << mat->b1 << ", " << mat->b2 << ", " << mat->b3 << ", " << mat->b4 << "\n";
//    std::cout << mat->c1 << ", " << mat->c2 << ", " << mat->c3 << ", " << mat->c4 << "\n";
//    std::cout << mat->d1 << ", " << mat->d2 << ", " << mat->d3 << ", " << mat->d4 << "\n";
//}